package org.noote.libs.spidy.anim;

import org.noote.libs.robot.wild.WildRobot_Animation;
import org.noote.libs.robot.wild.WildRobot_AnimationContext;

public class Spidy_WildAnim_Walk_Tripod extends WildRobot_Animation {
	
	protected float fStepTime = 0.0f;
	protected int iStep = 0;
	
	protected boolean bDoPlacement = false;
	float fServoOldPos[] = new float [6];
	
	// angle reduction for extremity legs 
	float fBodyCheatAngle = 25;
	
	float fStepDuration = 0.25f;
	
	float fCoxyAngle = 15;
	float fCoxyLow = 0, fCoxyHigh = 0;
	float fCoxyRightFactor = 0, fCoxyLeftFactor = 0;
	float fFemurLow = 20, fFemurHigh = 65;
	float fTibiaLow = 40, fTibiaHigh = 75;
	
	public Spidy_WildAnim_Walk_Tripod()
	{
		super("Walk 3P");
	}
	
	@Override
	public boolean create() {

		// ?
		
		return true;
	}
	
	float lerp(float min, float max, float lerp)
	{
		lerp = (lerp>1.0f)?1.0f:((lerp<0.0f)?0.0f:lerp); // clamp [0,1]
		return min + lerp*(max-min);
	}
	

	@Override
	public boolean startAnimation(WildRobot_AnimationContext context)
	{
		// reset anim step
		fStepTime = 0.0f;
		iStep = 0;
		
		float fFrontSpeed = context.getFactor(Spidy_WildRobot.FACTOR_SPEED_FRONT).getValue();
		float fRotation = context.getFactor(Spidy_WildRobot.FACTOR_ROTATION).getValue();
		
		fRotation = fRotation>1?1:fRotation<-1?-1:fRotation;
		if(fRotation != 0.0f) // not neutral ?
		{
			// have speed ?
			if(fFrontSpeed==0.0f) fFrontSpeed = 1.0f;
			fCoxyRightFactor = -fRotation*fFrontSpeed;
			fCoxyLeftFactor = fRotation*fFrontSpeed;
		}
		else
		{
			fCoxyRightFactor = 1.0f;
			fCoxyLeftFactor = 1.0f;
		}

		fFrontSpeed = (fFrontSpeed>1.0f)?1.0f:((fFrontSpeed<-1.0f)?-1.0f:fFrontSpeed);
		fCoxyLow = -fCoxyAngle*fFrontSpeed;
		fCoxyHigh = fCoxyAngle*fFrontSpeed;
		
		
		// here need to check is legs are already in position, else we need to add steps for reach first position as transition !
		bDoPlacement = checkPlacement(context);
		
		return !doAnimation(0, context);
	}

	@Override
	public boolean doAnimation(float fSlice, WildRobot_AnimationContext context)
	{
		if(bDoPlacement)
		{
			if(animPlacement(fSlice, context))
				bDoPlacement = false;
			return false;
		}
		return animTripod(fSlice, context);
	}
	
	public boolean animTripod(float fSlice, WildRobot_AnimationContext context)
	{
		float fCoxyT1 = fCoxyHigh, fFemurT1 = fFemurLow, fTibiaT1 = fTibiaLow;
		float fCoxyT2 = fCoxyLow, fFemurT2 = fFemurLow, fTibiaT2 = fTibiaLow;
		
		fStepTime+=fSlice;
		
		float fStep = (fStepTime>fStepDuration)?1:fStepTime/fStepDuration;
		
		// if no move, skip :)
		if(fCoxyLow!=0.0f && fCoxyHigh!=0.0f)
		{
			// ok ! do animation
			switch(iStep)
			{
			case 0: // leve pattes
				fCoxyT1 = lerp(fCoxyHigh, 0, fStep);
				fFemurT1 = fFemurLow;
				fTibiaT1 = fTibiaLow;
				fCoxyT2 = lerp(fCoxyLow, 0, fStep);
				fFemurT2 = lerp(fFemurLow, fFemurHigh, fStep);
				fTibiaT2 = lerp(fTibiaLow, fTibiaHigh, fStep);			
				break;
			case 1: // baisse pattes
				fCoxyT1 = lerp(0, fCoxyLow, fStep);
				fFemurT1 = fFemurLow;
				fTibiaT1 = fTibiaLow;
				fCoxyT2 = lerp(0, fCoxyHigh, fStep);			
				fFemurT2 = lerp(fFemurHigh, fFemurLow, fStep);
				fTibiaT2 = lerp(fTibiaHigh, fTibiaLow, fStep);			
				break;
			case 2: // leve pattes
				fCoxyT1 = lerp(fCoxyLow, 0, fStep);
				fFemurT1 = lerp(fFemurLow, fFemurHigh, fStep);
				fTibiaT1 = lerp(fTibiaLow, fTibiaHigh, fStep);			
				fCoxyT2 = lerp(fCoxyHigh, 0, fStep);
				fFemurT2 = fFemurLow;
				fTibiaT2 = fTibiaLow;
				break;
			case 3: // baisse pattes		
				fCoxyT1 = lerp(0, fCoxyHigh, fStep);			
				fFemurT1 = lerp(fFemurHigh, fFemurLow, fStep);
				fTibiaT1 = lerp(fTibiaHigh, fTibiaLow, fStep);			
				fCoxyT2 = lerp(0, fCoxyLow, fStep);
				fFemurT2 = fFemurLow;
				fTibiaT2 = fTibiaLow;
				break;
			default:
			}
			
			setTripodLegs(context, fCoxyT1, fFemurT1, fTibiaT1, fCoxyT2, fFemurT2, fTibiaT2);
		}
		
		// next step
		if(fStepTime>=fStepDuration)
		{
			fStepTime = 0.0f;
			iStep++;

			// end animation ?
			if(iStep>3) return true;
		}
		
		return false;
	}

	void setTripodLegs(WildRobot_AnimationContext context,
			float fCoxyT1, float fFemurT1, float fTibiaT1,
			float fCoxyT2, float fFemurT2, float fTibiaT2)
	{
		// left legs
		context.setPosition(Spidy_BaseSkeleton.LEFT_FRONT_COXA,   fBodyCheatAngle-(fCoxyT1*fCoxyLeftFactor));
		context.setPosition(Spidy_BaseSkeleton.LEFT_FRONT_FEMUR,  -fFemurT1);
		context.setPosition(Spidy_BaseSkeleton.LEFT_FRONT_TIBIA,  -fTibiaT1);

		context.setPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_COXA,   -(fCoxyT2*fCoxyLeftFactor));
		context.setPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_FEMUR,  -fFemurT2);
		context.setPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_TIBIA,  -fTibiaT2);

		context.setPosition(Spidy_BaseSkeleton.LEFT_BACK_COXA,   -fBodyCheatAngle-(fCoxyT1*fCoxyLeftFactor));
		context.setPosition(Spidy_BaseSkeleton.LEFT_BACK_FEMUR,  -fFemurT1);
		context.setPosition(Spidy_BaseSkeleton.LEFT_BACK_TIBIA,  -fTibiaT1);

		// right legs
		context.setPosition(Spidy_BaseSkeleton.RIGHT_FRONT_COXA,   -fBodyCheatAngle+(fCoxyT2*fCoxyRightFactor));
		context.setPosition(Spidy_BaseSkeleton.RIGHT_FRONT_FEMUR,  fFemurT2);
		context.setPosition(Spidy_BaseSkeleton.RIGHT_FRONT_TIBIA,  fTibiaT2);
		
		context.setPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_COXA,   (fCoxyT1*fCoxyRightFactor));
		context.setPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_FEMUR,  fFemurT1);
		context.setPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_TIBIA,  fTibiaT1);
		
		context.setPosition(Spidy_BaseSkeleton.RIGHT_BACK_COXA,   fBodyCheatAngle+(fCoxyT2*fCoxyRightFactor));
		context.setPosition(Spidy_BaseSkeleton.RIGHT_BACK_FEMUR,  fFemurT2);
		context.setPosition(Spidy_BaseSkeleton.RIGHT_BACK_TIBIA,  fTibiaT2);
	}

	protected boolean checkPlacement(WildRobot_AnimationContext context)
	{
		float fCoxyT1 = fCoxyHigh, fFemurT1 = fFemurLow, fTibiaT1 = fTibiaLow;
		float fCoxyT2 = fCoxyLow, fFemurT2 = fFemurLow, fTibiaT2 = fTibiaLow;

		if(context.getPosition(Spidy_BaseSkeleton.LEFT_FRONT_COXA) != fBodyCheatAngle-(fCoxyT1*fCoxyLeftFactor)) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.LEFT_FRONT_FEMUR) != -fFemurT1) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.LEFT_FRONT_TIBIA) != -fTibiaT1) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_COXA) != -(fCoxyT2*fCoxyLeftFactor)) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_FEMUR) != -fFemurT2) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_TIBIA) != -fTibiaT2) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.LEFT_BACK_COXA) != -fBodyCheatAngle-(fCoxyT1*fCoxyLeftFactor)) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.LEFT_BACK_FEMUR) != -fFemurT1) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.LEFT_BACK_TIBIA) != -fTibiaT1) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_FRONT_COXA) != -fBodyCheatAngle+(fCoxyT2*fCoxyRightFactor)) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_FRONT_FEMUR) != fFemurT2) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_FRONT_TIBIA) != fTibiaT2) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_COXA) != (fCoxyT1*fCoxyRightFactor)) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_FEMUR) != fFemurT1) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_TIBIA) != fTibiaT1) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_BACK_COXA) != fBodyCheatAngle+(fCoxyT2*fCoxyRightFactor)) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_BACK_FEMUR) != fFemurT2) return true;
		else if(context.getPosition(Spidy_BaseSkeleton.RIGHT_BACK_TIBIA) != fTibiaT2) return true;
		
		return false;
	}
	public boolean animPlacement(float fSlice, WildRobot_AnimationContext context)
	{
		float fStepPlacementDuration = 0.125f;
		
		if(fStepTime == 0)
		{
			switch(iStep)
			{
			case 0:
				fServoOldPos[0] = context.getPosition(Spidy_BaseSkeleton.LEFT_FRONT_COXA);
				fServoOldPos[1] = context.getPosition(Spidy_BaseSkeleton.LEFT_FRONT_FEMUR);
				fServoOldPos[2] = context.getPosition(Spidy_BaseSkeleton.LEFT_FRONT_TIBIA);
				fServoOldPos[3] = context.getPosition(Spidy_BaseSkeleton.RIGHT_BACK_COXA);
				fServoOldPos[4] = context.getPosition(Spidy_BaseSkeleton.RIGHT_BACK_FEMUR);
				fServoOldPos[5] = context.getPosition(Spidy_BaseSkeleton.RIGHT_BACK_TIBIA);
				break;
			case 2:
				fServoOldPos[0] = context.getPosition(Spidy_BaseSkeleton.LEFT_BACK_COXA);
				fServoOldPos[1] = context.getPosition(Spidy_BaseSkeleton.LEFT_BACK_FEMUR);
				fServoOldPos[2] = context.getPosition(Spidy_BaseSkeleton.LEFT_BACK_TIBIA);
				fServoOldPos[3] = context.getPosition(Spidy_BaseSkeleton.RIGHT_FRONT_COXA);				
				fServoOldPos[4] = context.getPosition(Spidy_BaseSkeleton.RIGHT_FRONT_FEMUR);
				fServoOldPos[5] = context.getPosition(Spidy_BaseSkeleton.RIGHT_FRONT_TIBIA);
				break;
			case 4:
				fServoOldPos[0] = context.getPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_COXA);
				fServoOldPos[1] = context.getPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_FEMUR);
				fServoOldPos[2] = context.getPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_TIBIA);
				fServoOldPos[3] = context.getPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_COXA);				
				fServoOldPos[4] = context.getPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_FEMUR);
				fServoOldPos[5] = context.getPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_TIBIA);
				break;
			}
		}
		
		fStepTime+=fSlice;
		
		float fStep = (fStepTime>fStepPlacementDuration)?1:fStepTime/fStepPlacementDuration;
		
		switch(iStep)
		{
		case 0:
			context.setPosition(Spidy_BaseSkeleton.LEFT_FRONT_FEMUR,  lerp(fServoOldPos[1], -fFemurHigh, fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_FRONT_TIBIA,  lerp(fServoOldPos[2], -fTibiaHigh, fStep));
			
			context.setPosition(Spidy_BaseSkeleton.RIGHT_BACK_FEMUR,  lerp(fServoOldPos[4], fFemurHigh, fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_BACK_TIBIA,  lerp(fServoOldPos[5], fTibiaHigh, fStep));
			break;
		case 1:
			context.setPosition(Spidy_BaseSkeleton.LEFT_FRONT_COXA,   lerp(fServoOldPos[0], fBodyCheatAngle-(fCoxyHigh*fCoxyLeftFactor), fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_FRONT_FEMUR,  lerp(-fFemurHigh, -fFemurLow, fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_FRONT_TIBIA,  lerp(-fTibiaHigh, -fTibiaLow, fStep));
			
			context.setPosition(Spidy_BaseSkeleton.RIGHT_BACK_COXA,   lerp(fServoOldPos[3], fBodyCheatAngle+(fCoxyLow*fCoxyRightFactor), fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_BACK_TIBIA,  lerp(fTibiaHigh, fTibiaLow, fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_BACK_FEMUR,  lerp(fFemurHigh, fFemurLow, fStep));
			break;
		case 2:
			context.setPosition(Spidy_BaseSkeleton.LEFT_BACK_FEMUR,  lerp(fServoOldPos[1], -fFemurHigh, fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_BACK_TIBIA,  lerp(fServoOldPos[2], -fTibiaHigh, fStep));
			
			context.setPosition(Spidy_BaseSkeleton.RIGHT_FRONT_FEMUR,  lerp(fServoOldPos[4], fFemurHigh, fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_FRONT_TIBIA,  lerp(fServoOldPos[5], fTibiaHigh, fStep));
			break;
		case 3:
			context.setPosition(Spidy_BaseSkeleton.LEFT_BACK_COXA,   lerp(fServoOldPos[0], -fBodyCheatAngle-(fCoxyHigh*fCoxyLeftFactor), fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_BACK_FEMUR,  lerp(-fFemurHigh, -fFemurLow, fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_BACK_TIBIA,  lerp(-fTibiaHigh, -fTibiaLow, fStep));
			
			context.setPosition(Spidy_BaseSkeleton.RIGHT_FRONT_COXA,   lerp(fServoOldPos[3], -fBodyCheatAngle+(fCoxyLow*fCoxyRightFactor), fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_FRONT_FEMUR,  lerp(fFemurHigh, fFemurLow, fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_FRONT_TIBIA,  lerp(fTibiaHigh, fTibiaLow, fStep));			
			break;
		case 4:
			context.setPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_FEMUR,  lerp(fServoOldPos[1], -fFemurHigh, fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_TIBIA,  lerp(fServoOldPos[2], -fTibiaHigh, fStep));
			
			context.setPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_FEMUR,  lerp(fServoOldPos[4], fFemurHigh, fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_TIBIA,  lerp(fServoOldPos[5], fTibiaHigh, fStep));
			break;
		case 5:
			context.setPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_COXA,   lerp(fServoOldPos[0], -(fCoxyLow*fCoxyLeftFactor), fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_FEMUR,  lerp(-fFemurHigh, -fFemurLow, fStep));
			context.setPosition(Spidy_BaseSkeleton.LEFT_MIDDLE_TIBIA,  lerp(-fTibiaHigh, -fTibiaLow, fStep));

			context.setPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_COXA,   lerp(fServoOldPos[3], (fCoxyHigh*fCoxyRightFactor), fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_FEMUR,  lerp(fFemurHigh, fFemurLow, fStep));
			context.setPosition(Spidy_BaseSkeleton.RIGHT_MIDDLE_TIBIA,  lerp(fTibiaHigh, fTibiaLow, fStep));			
			break;
		}
		
		// next step
		if(fStepTime>=fStepPlacementDuration)
		{
			fStepTime = 0.0f;
			iStep++;

			// end animation ?
			if(iStep>5) return true;
		}
		
		return false;
	}
}
